#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import serial
import os, sys
import rospy
import time
from std_msgs.msg import Int16
from PyCRC.CRC16 import CRC16


class GripperCtrl(object):
    def __init__(self):
        rospy.init_node('gripperctrl')
        self.__ser = serial.Serial('/dev/abotbase', 115200, timeout=2)
        if self.__ser.isOpen():
            self.__ser.close()
        try:
            self.__ser.open()
        except serial.SerialException as msg:
            print ("Serial open failed!")
            sys.exit()

        self.crc_modbus = CRC16(modbus_flag=True)

        self.gripper_topic = rospy.Subscriber("gripper_ctrl", Int16, self.GripperCallback)

        time.sleep(1)
        self.init_gripper()
        time.sleep(1)

    def write_data(self, data):
        sdata = [0x01, 0x06, 0x01, 0x03]
        sdata.append((data>>8)&0xff)
        sdata.append(data&0xff)
        crc_data = self.crc_modbus.calculate(bytes(sdata))
        sdata.append(crc_data&0xff)
        sdata.append((crc_data>>8)&0xff)
        self.__ser.write(bytes(sdata)) 
        print(bytes(sdata))

    def init_gripper(self):
        init_data = '01 06 01 00 00 A5 48 4D'
        data = bytes.fromhex(init_data)
        self.__ser.write(bytes(data))
        print(data)
    
    
    def GripperCallback(self, msg):
        self.write_data(msg.data)




if __name__ == '__main__':
    try:
        gripper = GripperCtrl()
        rospy.spin()
    except Exception as e:
        rospy.logerr("%s", str(e))
        os._exit(1)






